import math

from ship_envs.core.ship import Ship
import numpy as np


class ShipActionContinus(Ship):
    def __init__(self):
        super().__init__()
        self.cur_rotate_speed = 0
        self.max_rotate_speed = 1
        self.min_rotate_speed = -1
        self.turn_zero = 0.2

    def move_by_action(self, action):
        if -0.1 < action < 0.1:
            # 防止抖动
            action = 0
        # 添加自动回正功能
        if action == 0:
            if self.cur_rotate_speed > 0:
                self.cur_rotate_speed = max(0, self.cur_rotate_speed - self.turn_zero)
            elif self.cur_rotate_speed < 0:
                self.cur_rotate_speed = min(0, self.cur_rotate_speed + self.turn_zero)
        else:
            self.cur_rotate_speed += action
        self.cur_rotate_speed = np.clip(self.cur_rotate_speed, self.min_rotate_speed, self.max_rotate_speed)

        # 暂时按离散动作进行测试
        if self.cur_rotate_speed != 0:
            self.model.rotate(self.cur_rotate_speed * self.dt)
        self.model.move(self.cur_speed * self.dt)
